#ifndef PROJECT_UTILS_OCCUPANCY_CLUSTER_H
#define PROJECT_UTILS_OCCUPANCY_CLUSTER_H

#include <clustering_speed_test/cluster_base.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/PointIndices.h>

#include <queue>


class Occupancy_cluster : public Cluster_base{
 public:
  struct Occupancy_cluster_config {
    Occupancy_cluster_config(const double _max_x,const double _min_x,
                             const double _max_y,const double _min_y,
                             double _grid_res,int _BFS_puff_size) {
      max_x = _max_x; min_x = _min_x;
      max_y = _max_y; min_y = _min_y;
      grid_res = _grid_res;
      BFS_puff_size = _BFS_puff_size;
      is_config_valid = true;
    }
    Occupancy_cluster_config() {}
    double max_x,min_x;
    double min_y,max_y;
    int BFS_puff_size;
    double grid_res;

    bool is_config_valid = false;
  };
  Occupancy_cluster(ros::NodeHandle &_nh,YAML::Node& _node):Cluster_base(_nh,_node){
    // nh_ = _nh;
    // sub_cloud_ = nh_.subscribe<sensor_msgs::PointCloud2>("/rslidar_points",100,&Occupancy_cluster::cloud_callback,this);
  }
  void clustering(const PointCloudPtr _cloud,std::vector<pcl::PointIndices>& _clustering_indices) override;
  void clustering_(const double _grid_size,const int _BFS_puff_size ,const PointCloudPtr _cloud,std::vector<pcl::PointIndices>& _clustering_indices);
  void clustering_(const Occupancy_cluster_config _config,const PointCloudPtr _cloud,std::vector<pcl::PointIndices>& _clustering_indices);
 private:
  void BEV_project_cloud_and_clustering(PointCloudPtr _cloud);
  void cal_cloud_max_min_x_y(PointCloudPtr _cloud);
  void init_grid_matrix();
  bool cal_clustering_index(const double _point_x,const double _point_y,int& _matrix_x,int& _matrix_y);
  void BFS_clustering();
  std::vector<std::pair<int,int>> BFS_from_here(int _y_index,int _x_index);
 private:
  Occupancy_cluster_config clustering_config;
  std::vector<std::vector<int>> clustering_matrix;
  std::vector<std::vector<std::vector<int>>> every_grid_points_indices;
  std::vector<pcl::PointIndices> clustered_indices;
  std::vector<std::pair<int,int>> BFS_puff_seeds;

  int grid_height;
  int grid_width;
  int occupied_th = 2;
  int clustering_nums = 0;
};

#endif